/*
 * 任务2：Scout小车运动控制
 * 作者：车载软件作业
 * 功能：控制小车前进、后退和旋转
 */

#include <iostream>
#include <iomanip>
#include <unistd.h>
#include <ros/ros.h>
#include <termios.h>
#include "ugv_sdk/scout/scout_base.hpp" //使用小车上的sdk版本

using namespace westonrobot;

// 定义一个模拟 getch() 的函数
int getch() {
    struct termios oldt, newt;
    int ch;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
    ch = fgetc(stdin);
    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
    return ch;
}

// 打印分隔线
void printSeparator() {
    std::cout << "============================================================" << std::endl;
}

// 打印运动状态
void printMotionStatus(ScoutBase& scout) {
    auto state = scout.GetScoutState();
    std::cout << "线速度: " << std::fixed << std::setprecision(2) 
              << state.linear_velocity << " m/s, ";
    std::cout << "角速度: " << std::fixed << std::setprecision(2) 
              << state.angular_velocity << " rad/s" << std::endl;
}

// 等待并显示状态
void waitAndShowStatus(ScoutBase& scout, int seconds) {
    for (int i = 0; i < seconds; i++) {
        printMotionStatus(scout);
    }
}

int main(int argc, char **argv) {
    std::cout << "\n";
    printSeparator();
    std::cout << "         任务2：Scout 2.0 小车运动控制系统" << std::endl;
    printSeparator();
    std::cout << "\n";
    
    // ============== 1. 连接小车 ==============
    std::string can_device = "can0";
    if (argc == 2) {
        can_device = argv[1];
    }
    
    std::cout << "► 正在连接CAN设备: " << can_device << std::endl;
    int32_t baud_rate = 0;
    ScoutBase scout;
    scout.Connect(can_device, baud_rate);

    int flag = true;
    while(flag){
        // 获取用户输入
        int key = getch();
        switch (key){
            case 'w':
                scout.SetMotionCommand(2.0, 0.0);
                break;
            case 's':
                scout.SetMotionCommand(-2.0, 0.0);
                break;
            case 'a':
                scout.SetMotionCommand(0.0, 0.5);
                break;
            case 'd':
                scout.SetMotionCommand(0.0, -0.5);
                break;
            case 'q':
                scout.SetMotionCommand(0.0, 0.0);
                flag = false;
                break;
            default:
                scout.SetMotionCommand(0.0, 0.0);
                break;
        }

        waitAndShowStatus(scout, 1);
    }
    return 0;
}

